#include "imu.h"

raw_t raw = {0};   
uint8_t rx_buf[10];

static int sync_ch(uint8_t *buf, uint8_t data)
{
    buf[0] = buf[1];
    buf[1] = data;
    return buf[0] == CHSYNC1 && buf[1] == CHSYNC2;
}

static void crc16_update(uint16_t *currect_crc, const uint8_t *src, uint32_t len)
{
    uint32_t crc = *currect_crc;
    uint32_t j;
    for (j=0; j < len; ++j)
    {
        uint32_t i;
        uint32_t byte = src[j];
        crc ^= byte << 8;
        for (i = 0; i < 8; ++i)
        {
            uint32_t temp = crc << 1;
            if (crc & 0x8000)
            {
                temp ^= 0x1021;
            }
            crc = temp;
        }
    } 
    *currect_crc = crc;
}

static int parse_data(raw_t *raw)
{
    uint8_t *p = &raw->buf[CH_HDR_SIZE];
    /*memset(raw->item_code, 0, sizeof(raw->item_code));
    raw->nitem_code = 0;*/
    raw->imu.id = U1(p+1);
    raw->imu.temperature = U1(p+3);
    raw->imu.pressure = R4(p+4);
    raw->imu.timestamp = U4(p+8);
    raw->imu.acc[0] = R4(p+12);
    raw->imu.acc[1] = R4(p+16);
    raw->imu.acc[2] = R4(p+20);
    raw->imu.gyr[0] = R4(p+24);
    raw->imu.gyr[1] = R4(p+28);
    raw->imu.gyr[2] = R4(p+32);
    raw->imu.mag[0] = R4(p+36);
    raw->imu.mag[1] = R4(p+40);
    raw->imu.mag[2] = R4(p+44);
    raw->imu.eul[0] = R4(p+48);
    raw->imu.eul[1] = R4(p+52);
    raw->imu.eul[2] = R4(p+56);
    raw->imu.quat[0] = R4(p+60);
    raw->imu.quat[1] = R4(p+64);
    raw->imu.quat[2] = R4(p+68);
    raw->imu.quat[3] = R4(p+72);
    Robot.Robot_in_self.position_now.yaw=raw->imu.eul[2] / 180.0f * PI;	
    return 1;
}

static int decode_ch(raw_t *raw)
{
    uint16_t crc = 0;   

    /* checksum */
    crc16_update(&crc, raw->buf, 4);
    crc16_update(&crc, raw->buf+6, raw->len);
    if (crc != U2(raw->buf+4))
    {
       // CH_TRACE("ch checksum error: frame:0x%X calcuate:0x%X, len:%d\n", U2(raw->buf+4), crc, raw->len);
        return -1;
    }
    
    return parse_data(raw);
}

int ch_serial_input(raw_t *raw, uint8_t data)
{
    /* synchronize frame */
    if (raw->nbyte == 0)
    {
        if (!sync_ch(raw->buf, data)) return 0;
        raw->nbyte = 2;
        return 0;
    }

    raw->buf[raw->nbyte++] = data;
    
    if (raw->nbyte == CH_HDR_SIZE)
    {
        if ((raw->len = U2(raw->buf+2)) > (MAXRAWLEN - CH_HDR_SIZE))
        {
            //CH_TRACE("ch length error: len=%d\n",raw->len);
            raw->nbyte = 0;
            return -1;
        }
    }
    
    if (raw->nbyte < (raw->len + CH_HDR_SIZE)) 
    {
        return 0;
    }
    
    raw->nbyte = 0;
    
    return decode_ch(raw);
}
